#include "listener.hpp"
// mars_rover::listener::ListenerNode
namespace mars_rover
{
    namespace listener
    {
        ListenerNode::ListenerNode(const rclcpp::NodeOptions &options) : rclcpp::Node("listener_node",options)
        {
            RCLCPP_INFO(this->get_logger(), "listener结点创建成功！");
            sub_image = this->create_subscription<sensor_msgs::msg::Image>("image_raw",1,std::bind(&ListenerNode::topic_image_callback,this,std::placeholders::_1));
            sub_pointCloud2 = this->create_subscription<sensor_msgs::msg::PointCloud2>("pointcloud_raw",1,std::bind(&ListenerNode::topic_pointcloud_callback,this,std::placeholders::_1));
            static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
            sub_pose = this->create_subscription<geometry_msgs::msg::PoseStamped>("pose_raw",1,std::bind(&ListenerNode::topic_pose_callback,this,std::placeholders::_1));
            // load parameters
            // source_frame_ = declare_parameter<std::string>("source_frame", "source_frame");
            target_frame_ = declare_parameter<std::string>("target_frame", "target_frame");

            /* create subsribers */
            tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
            tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        }
        void ListenerNode::topic_image_callback(const sensor_msgs::msg::Image::SharedPtr msg) 
        {
            RCLCPP_INFO(this->get_logger(), "图像接收结点成功！");
            cv::Mat img;
            cv_bridge::CvImagePtr cv_ptr;
            cv_ptr = cv_bridge::toCvCopy(msg,"bgr8");
            cv_ptr->image.copyTo(img);
            RCLCPP_INFO(this->get_logger(), "此处进行图像展示！");
            // cv::imshow("myHello",img);
            // cv::waitKey(3);
        }
        void ListenerNode::topic_pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
        {
            // pcl::PointCloud<pcl::PointXYZI>::Ptr map;
            RCLCPP_INFO(this->get_logger(), "点云接收结点成功！");
            pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
            pcl::fromROSMsg(*msg,*cloud);
            pcl::io::savePCDFileASCII("listener.pcd",*cloud);
            //如何进行点云展示呢？保存代替pcd展示，说明已经接收到了数据
        }
        void ListenerNode::topic_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
        {
            RCLCPP_INFO(this->get_logger(), "tf和pose信息接收成功！");
            source_point_.header.frame_id = "source_frame_";
            source_point_.header.stamp = this->now();
            source_point_.point.x = msg->pose.position.x;
            source_point_.point.y = msg->pose.position.y;
            source_point_.point.z = msg->pose.position.z;
            std::cout << "source position: (" << source_point_.point.x << ", " << source_point_.point.y << ", " << source_point_.point.z << ")" << std::endl;
            target_point_ = tf_listener_->transform(source_point_, target_frame_);
            std::cout << "target position: (" << target_point_.point.x << ", " << target_point_.point.y << ", " << target_point_.point.z << ")" << std::endl;
            static_tf_broadcaster_->sendTransform(target_point_);
        }
    }
}
RCLCPP_COMPONENTS_REGISTER_NODE(mars_rover::listener::ListenerNode)
